#include "stdafx.h"

// Globals
CvMemStorage* storage;
CvHaarClassifierCascade*  cascade;
CvBGStatModel* bgmodel;
UdpTransmitSocket *transmitSocket;

// Function prototypes
void DetectAndSend(IplImage*);

int _tmain(int argc, _TCHAR* argv[]) {

    transmitSocket = new UdpTransmitSocket(IpEndpointName(ADDRESS, PORT));

    IplImage *frame, *frame_copy = NULL;
	bgmodel = NULL;
    cascade = (CvHaarClassifierCascade*)cvLoad("haarcascade_frontalface_alt.xml", 0, 0, 0);
    if( !cascade ) {
        fprintf( stderr, "ERROR: Could not load classifier cascade\n" );
        return -1;
    }
    storage = cvCreateMemStorage(0);
    CvCapture* capture = cvCaptureFromCAM(0);
    cvNamedWindow( "face detection", 1);
	cvNamedWindow("background model", 1);

	// throw away the first 20 frames
	for (int i=0; i<100; i++) {
		frame = cvQueryFrame(capture);
		cvWaitKey(1);
	}
	if (frame) {
		if (!frame_copy) frame_copy = cvCloneImage(frame);
		if(frame->origin == IPL_ORIGIN_TL) {
			cvCopy( frame, frame_copy, 0);
		} else {
			cvFlip( frame, frame_copy, 0 );
		}
		if (!bgmodel) bgmodel = cvCreateFGDStatModel(frame_copy);
	}


    if(capture) {
        while(true) {
            frame = cvQueryFrame(capture);
            if (!frame) break;
            if(frame->origin == IPL_ORIGIN_TL) {
                cvCopy( frame, frame_copy, 0);
            } else {
                cvFlip( frame, frame_copy, 0 );
            }
			cvUpdateBGStatModel(frame_copy, bgmodel);

			DetectAndSend(frame_copy);
            if( cvWaitKey(10) >= 0) break;
        }
        cvReleaseImage( &frame_copy );
        cvReleaseCapture( &capture );
    }
    cvDestroyWindow("face detection");
    cvDestroyWindow("background model");
    delete transmitSocket;
    
    return 0;
}

void DetectAndSend(IplImage *img) {

    char buffer[OUTPUT_BUFFER_SIZE];
    osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
	char blobInfo[OUTPUT_BUFFER_SIZE];

    double scale = 1.3;
    IplImage* gray = cvCreateImage( cvSize(img->width,img->height), 8, 1);
    IplImage* small_img = cvCreateImage( cvSize( cvRound (img->width/scale),
        cvRound (img->height/scale)), 8, 1 );

    cvCvtColor( img, gray, CV_BGR2GRAY );
    cvResize( gray, small_img, CV_INTER_LINEAR );
    cvEqualizeHist( small_img, small_img );
    cvClearMemStorage( storage );

	p << osc::BeginBundleImmediate;

	if (bgmodel) {
		CvSeq* contours = NULL;
		cvFindContours(bgmodel->foreground, storage, &contours, sizeof(CvContour), CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
		if (contours != NULL) {
			int blobNum =0;
			
			contours = cvApproxPoly(contours, sizeof(CvContour), storage, CV_POLY_APPROX_DP, 3, 1 );
			for (CvSeq *contour = contours; contour != NULL; contour = contour->h_next) {
				double ctsize = fabs(cvArcLength(contour));
//				printf("found foreground contour of length %f\n, ctsize");
				if ((ctsize > BODY_MIN_SIZE) && (ctsize < BODY_MAX_SIZE)) {
					CvRect r = cvBoundingRect(contour, 1);
					CvPoint center;
					int radius;
					center.x = cvRound((r.x + r.width*0.5));
					center.y = cvRound((r.y + r.height*0.5));
					radius = cvRound((r.width + r.height)*0.25);
					cvCircle( img, center, radius, CV_RGB(0,255,0), 3, 8, 0 );

					p << osc::BeginMessage( "/blob" ) << blobNum++
						<< center.x << center.y << radius << osc::EndMessage;
				}
			}
			int blobs= contours->total;
			p << osc::BeginMessage("/blobs") << blobNum << osc::EndMessage;

		}
	}
    cvClearMemStorage( storage );

    if( cascade )
    {
        double t = (double)cvGetTickCount();
        CvSeq* faces = cvHaarDetectObjects( small_img, cascade, storage,
                                            1.1, 2, 0/*CV_HAAR_DO_CANNY_PRUNING*/,
                                            cvSize(30, 30) );
        t = (double)cvGetTickCount() - t;
        printf( "detection time = %gms\n", t/((double)cvGetTickFrequency()*1000.) );

        for(int i = 0; i < (faces ? faces->total : 0); i++ )
        {
            CvRect* r = (CvRect*)cvGetSeqElem( faces, i );
            CvPoint center;
            int radius;
            center.x = cvRound((r->x + r->width*0.5)*scale);
            center.y = cvRound((r->y + r->height*0.5)*scale);
            radius = cvRound((r->width + r->height)*0.25*scale);
            cvCircle( img, center, radius, CV_RGB(255,0,0), 3, 8, 0 );

            p << osc::BeginMessage( "/face" ) << i
                << center.x << center.y << radius << osc::EndMessage;

        }
			p << osc::BeginMessage("/faces") << (faces ? faces->total : 0) 
			<< osc::EndMessage;


    }
    p << osc::EndBundle;
    
    transmitSocket->Send( p.Data(), p.Size() );

    cvFlip(img,img,0);
    cvShowImage("face detection", img );
	cvShowImage("background model", bgmodel->background);
	cvReleaseImage( &gray );
    cvReleaseImage( &small_img ); 
}
